/*
 Software Serial
    by Cheng Maohua
*/
#ifndef SOFT_SERIAL_H
#define SOFT_SERIAL_H

#include <SoftwareSerial.h>
#include <ArduinoJson.h>

#include "component_ultrasonic.h"
#define CONFIG_SOFTSERIAL_DEBUG

// Motor action code
const char MOTOR_GO = 'G';
const char MOTOR_BACK = 'B';
const char MOTOR_STOP = 'S';
const char MOTOR_LEFT = 'L';
const char MOTOR_RIGHT = 'R';
const char MOTOR_UP = 'U';
const char MOTOR_DOWN = 'D';

// Softserial:  ESP8266 - Arduino
#define SS_RX_PIN 3  //   ESP8266 TX
#define SS_TX_PIN 11 //   ESP8266 RX

SoftwareSerial espSerial(SS_RX_PIN, SS_TX_PIN);

// Send data to esp8266
const int interval_send_data = 100;
long previousMillis_send_data;
StaticJsonDocument<200> sensor_json;

void softserial_cmd()
{
  // from esp8266 softserial
  String inString = "";
  while (espSerial.available())
  {
    inString += char(espSerial.read());
#ifdef CONFIG_SOFTSERIAL_DEBUG
    Serial.print(inString);
#endif
  };

  if (inString.indexOf(MOTOR_GO) != -1)
  {
    cur_car_cmd = CAR_FORWARD;
  }
  else if (inString.indexOf(MOTOR_STOP) != -1)
  {
    cur_car_cmd = CAR_STOP;
  }
  else if (inString.indexOf(MOTOR_BACK) != -1)
  {
    cur_car_cmd = CAR_BACKWARD;
  }
  else if (inString.indexOf(MOTOR_LEFT) != -1)
  {
    cur_car_cmd = CAR_LEFT;
  }
  else if (inString.indexOf(MOTOR_RIGHT) != -1)
  {
    cur_car_cmd = CAR_RIGHT;
  }
  else if (inString.indexOf(MOTOR_UP) != -1)
  {
    cur_car_cmd = CAR_SPEED_UP;
  }
  else if (inString.indexOf(MOTOR_DOWN) != -1)
  {
    cur_car_cmd = CAR_SPEED_DOWN;
  };
  Serial.print("car cmd: ");
  Serial.println(cur_car_cmd);
  car_action(cur_car_cmd);
}

void setup_softserial()
{
  espSerial.begin(9600);
  previousMillis_send_data = 0;
};

void loop_softserial()
{
  softserial_cmd();

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis_send_data >= interval_send_data)
  {
    previousMillis_send_data = currentMillis;
    sensor_json["distance"] = String(distance_cm, 0);
    serializeJson(sensor_json, espSerial);
  };
}

#endif /* SOFT_SERIAL_H */